﻿#include "Sm2.h"
#include "Random_Bytes.h"
#include <assert.h>
#include <stdio.h>


// 字节转化为字
#define GM_GETU32(p)           \
    ((uint32_t) (p)[0] << 24 | \
     (uint32_t) (p)[1] << 16 | \
     (uint32_t) (p)[2] << 8 |  \
     (uint32_t) (p)[3])

// 字转化为字节
#define GM_PUTU32(p, V)              \
    ((p)[0] = (uint8_t) ((V) >> 24), \
     (p)[1] = (uint8_t) ((V) >> 16), \
     (p)[2] = (uint8_t) ((V) >> 8),  \
     (p)[3] = (uint8_t) (V))

// SM2 P
const uint256_t GM_BN_P = {
        0xFFFFFFFF, 0xFFFFFFFF, 0x00000000, 0xFFFFFFFF,
        0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFE };

// SM2 N
const uint256_t GM_BN_N = {
        0x39D54123, 0x53BBF409, 0x21C6052B, 0x7203DF6B,
        0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFE };

// 蒙哥马利域转化用参数，(r ^ 2)(mod p)
static const uint256_t GM_BN_MONT_PRR = {
        0x00000003, 0x00000002, 0xFFFFFFFF, 0x00000002,
        0x00000001, 0x00000001, 0x00000002, 0x00000004 };

// 蒙哥马利域转化用参数，(r ^ 2)(mod n)
static const uint256_t GM_BN_MONT_NRR = {
        0x7C114F20, 0x901192AF, 0xDE6FA2FA, 0x3464504A,
        0x3AFFE0D4, 0x620FC84C, 0xA22B3D3B, 0x1EB5E412 };

// 大数0
static const uint256_t U256_0 = {
        0x0, 0x0, 0x0, 0x0,
        0x0, 0x0, 0x0, 0x0 };

// 大数1
static const uint256_t U256_1 = {
        0x1, 0x0, 0x0, 0x0,
        0x0, 0x0, 0x0, 0x0 };

// 大数2
static const uint256_t U256_2 = {
        0x2, 0x0, 0x0, 0x0,
        0x0, 0x0, 0x0, 0x0 };

// 蒙哥马利域 1 (mod p)
static const uint256_t GM_BN_MONT_PONE = {
        0x00000001, 0x00000000, 0xFFFFFFFF, 0x00000000,
        0x00000000, 0x00000000, 0x00000000, 0x00000001 };

// 蒙哥马利域 1 (mod n)
static const uint256_t GM_BN_MONT_NONE = {
        0xC62ABEDD, 0xAC440BF6, 0xDE39FAD4, 0x8DFC2094,
        0x00000000, 0x00000000, 0x00000000, 0x00000001 };

// 蒙哥马利域, SM2 G
static const u256_point_t GM_MONT_G = {
        {0xF418029E, 0x61328990, 0xDCA6C050, 0x3E7981ED, 0xAC24C3C3, 0xD6A1ED99, 0xE1C13B05, 0x91167A5E},
        {0x3C2D0DDD, 0xC1354E59, 0x8D3295FA, 0xC1F5E578, 0x6E2A48F8, 0x8D4CFB06, 0x81D735BD, 0x63CD65D4},
        {0x00000001, 0x00000000, 0xFFFFFFFF, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000001} };
const u256_point_t* GM_MONT_G_PTR = &GM_MONT_G;

// 蒙哥马利域, SM2 A
const uint256_t GM_BN_MONT_A = {
        0xFFFFFFFC, 0xFFFFFFFF, 0x00000003, 0xFFFFFFFC,
        0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFB };

// 蒙哥马利域, SM2 B
const uint256_t GM_BN_MONT_B = {
        0x2BC0DD42, 0x90D23063, 0xE9B537AB, 0x71CF379A,
        0x5EA51C3C, 0x52798150, 0xBA20E2C8, 0x240FE188 };

// 扩展大数加法，r = (a + b)
static void gm_i_bn_add_x(uint256_t r, const uint256_t a, const uint256_t b, int count) {
    int i;
    r[0] = a[0] + b[0];
    for (i = 1; i < count; i++) {
        r[i] = a[i] + b[i] + (r[i - 1] >> 32);
        r[i - 1] &= 0x0FFFFFFFFULL;
    }
}

// 大数减法，r = (a - b)
static void gm_i_bn_sub(uint256_t r, const uint256_t a, const uint256_t b) {
    int i;
    r[0] = 0x100000000ULL + a[0] - b[0];
    for (i = 1; i < U256_ARR_SIZE_SUB_ONE; i++) {
        r[i] = 0x0FFFFFFFFULL + a[i] - b[i] + (r[i - 1] >> 32);
        r[i - 1] &= 0x0FFFFFFFFULL;
    }
    r[i] = a[i] - b[i] + (r[i - 1] >> 32) - 1;
    r[i - 1] &= 0x0FFFFFFFFULL;
}

// 大数乘法, r = k[b]
static void gm_i_bn_mul(uint64_t* r, const uint64_t* k, const uint256_t b) {
    int i, j;
    // k0b0, k0b1+k1b0, .. ,k0b7+k1b6, k1b7
    uint64_t t;

    for (i = 0; i < 2; i++) {
        t = 0;
        for (j = 0; j < 8; j++) {
            t = r[i + j] + k[i] * b[j] + t;
            r[i + j] = t & 0x0FFFFFFFFULL;
            t >>= 32;
        }
        r[i + 8] = t;
    }
}

// 大数加法，r = (a + b)
static void gm_i_bn_add(uint256_t r, const uint256_t a, const uint256_t b) {
    gm_i_bn_add_x(r, a, b, 8);
}

// 十六进制转化为int
static int gm_hex2int(char c) {
    switch (c) {
    case '0':
    case '1':
    case '2':
    case '3':
    case '4':
    case '5':
    case '6':
    case '7':
    case '8':
    case '9':
        return c - '0';
    case 'a':
    case 'b':
    case 'c':
    case 'd':
    case 'e':
    case 'f':
        return c - 'a' + 10;
    case 'A':
    case 'B':
    case 'C':
    case 'D':
    case 'E':
    case 'F':
        return c - 'A' + 10;
    default:
        return -1;
    }
}

// 十六进制转化为二进制
int sm2_hex2bin(const char* in, int in_len, uint8_t* out) {
    int c;
    if ((in_len % 2) != 0) {
        return -1;
    }

    while (in_len) {
        if ((c = gm_hex2int(*in++)) < 0) {
            return -1;
        }
        *out = (uint8_t)c << 4;

        if ((c = gm_hex2int(*in++)) < 0) {
            return -1;
        }
        *out |= (uint8_t)c;

        in_len -= 2;
        out++;
    }
    return 1;
}

// 将大数转化为二进制串
void uint256_to_bytes(const uint256_t a, uint8_t out[32]) {
    int i;
    for (i = 7; i >= 0; i--) {
        GM_PUTU32(out, a[i]);
        out += 4;
    }
}

// 将二进制串转化为大数
void uint256_from_bytes(uint256_t r, const uint8_t in[32]) {
    int i;
    for (i = 7; i >= 0; i--) {
        r[i] = GM_GETU32(in);
        in += 4;
    }
}

// 将大数转化为十六进制串
void uint256_to_hex(const uint256_t a, char hex[64]) {
    int i;
    for (i = 7; i >= 0; i--) {
        int len;
        len = snprintf(hex, 8, "%08X", (uint32_t)a[i]);
        assert(len == 8);
        hex += 8;
    }
}

// 将十六进制串转化为大数
int uint256_from_hex(uint256_t r, const char hex[64]) {
    uint8_t buf[32];
    if (sm2_hex2bin(hex, 64, buf) < 0)
        return -1;
    uint256_from_bytes(r, buf);
    return 1;
}

// 将大数转化为比特串
void uint256_to_bits(const uint256_t a, char bits[256]) {
    int i, j;
    for (i = 7; i >= 0; i--) {
        uint64_t w = a[i];
        for (j = 0; j < 32; j++) {
            *bits++ = (w & 0x080000000ULL) ? '1' : '0';
            w <<= 1;
        }
    }
}

// 大数比较
int uint256_cmp(const uint256_t a, const uint256_t b) {
    for (int i = 7; i >= 0; i--) {
        if (a[i] > b[i])
            return 1;
        if (a[i] < b[i])
            return -1;
    }
    return 0;
}

// 大数模加，(a + b)(mod m)
void uint256_add(uint256_t r, const uint256_t a, const uint256_t b, const uint256_t m) {
    gm_i_bn_add(r, a, b);
    if (uint256_cmp(r, m) >= 0) {
        gm_i_bn_sub(r, r, m);
    }
}

// 大数模减，(a - b)(mod m)
void uint256_sub(uint256_t r, const uint256_t a, const uint256_t b, const uint256_t m) {
    if (uint256_cmp(a, b) >= 0) {
        gm_i_bn_sub(r, a, b);
    }
    else {
        uint256_t t;
        gm_i_bn_sub(t, m, b);
        gm_i_bn_add(r, t, a);
    }
}

// 蒙哥马利域模乘，(a * b)(mod m)
void uint256_mont_mul(uint256_t r, const uint256_t a, const uint256_t b, const uint256_t m) {
    uint64_t temp_r[11] = { 0 };
    uint64_t temp_mul[11] = { 0 };
    uint64_t temp_q[2] = { 0 };
    int i, j;

    uint64_t mod_t = (m[0] == GM_BN_N[0] ? 0x327F9E8872350975ULL : 0x01);

    for (i = 0; i < 4; i++) {
        // bi*a
        memset(temp_mul, 0, sizeof(uint64_t) * 11);
        gm_i_bn_mul(temp_mul, &b[i * 2], a);
        gm_i_bn_add_x(temp_r, temp_r, temp_mul, 11);

        // cal q
        uint64_t q = (temp_r[0] | temp_r[1] << 32) * mod_t;
        temp_q[0] = q & 0x0FFFFFFFFULL;
        temp_q[1] = q >> 32;

        // q * n
        memset(temp_mul, 0, sizeof(uint64_t) * 11);
        gm_i_bn_mul(temp_mul, temp_q, m);
        gm_i_bn_add_x(temp_r, temp_r, temp_mul, 11);

        // reduce temp_r
        memmove(temp_r, temp_r + 2, sizeof(uint64_t) * 9);
        temp_r[9] = 0;
        temp_r[10] = 0;
    }

    temp_r[7] += temp_r[8] << 32;
    if (uint256_cmp(temp_r, m) >= 0) {
        gm_i_bn_sub(temp_r, temp_r, m);
    }
    uint256_copy(r, temp_r);
}

/**
 * 将大数转化为蒙哥马利域
 * @param r 计算结果
 * @param a 大数a
 * @param m 模m
 */
void uint256_to_mont(uint256_t r, const uint256_t a, const uint256_t m) {
    uint256_mont_mul(r, a, (m[0] == GM_BN_N[0] ? GM_BN_MONT_NRR : GM_BN_MONT_PRR), m);
}

/**
 * 将蒙哥马利域转化为普通大数
 * @param r 计算结果
 * @param a 大数a
 * @param m 模m
 */
void uint256_from_mont(uint256_t r, const uint256_t a, const uint256_t m) {
    uint256_mont_mul(r, a, U256_1, m);
}

/**
 * 蒙哥马利域模平方，(a * a)(mod m)
 * @param r 计算结果
 * @param a 大数a
 * @param m 模m
 */
void uint256_sqr(uint256_t r, const uint256_t a, const uint256_t m) {
    uint256_mont_mul(r, a, a, m);
}

/**
 * 蒙哥马利域模幂，(a ^ b)(mod m)
 * @param r 计算结果
 * @param a 大数a
 * @param b 大数b
 * @param m 模m
 */
void uint256_exp(uint256_t r, const uint256_t a, const uint256_t b, const uint256_t m) {
    uint256_t t;
    uint64_t w;
    int i, j;

    // set t to mont one
    uint256_to_mont(t, U256_1, m);

    for (i = 7; i >= 0; i--) {
        w = b[i];
        for (j = 0; j < 32; j++) {
            uint256_sqr(t, t, m);
            if (w & 0x080000000ULL) {
                uint256_mont_mul(t, t, a, m);
            }
            w <<= 1;
        }
    }

    uint256_copy(r, t);
}

/**
 * 蒙哥马利域模逆，(a ^ -1)(mod m)
 * @param r 计算结果
 * @param a 大数a
 * @param m 模m
 */
void uint256_inv(uint256_t r, const uint256_t a, const uint256_t m) {
    uint256_t e;
    gm_i_bn_sub(e, m, U256_2);
    uint256_exp(r, a, e, m);
}

// 将大数设置为蒙哥马利域1
void uint256_set_mont_one(uint256_t r) {
    uint256_copy(r, GM_BN_MONT_PONE);
}

// 将大数设置为0
void gm_bn_set_zero(uint256_t r) {
    uint256_copy(r, U256_0);
}

/**
 * 判断大数是否为蒙哥马利域1
 * @return 1为true，否则为false
 */
int uint256_is_mont_one(const uint256_t r) {
    return memcmp(r, GM_BN_MONT_PONE, sizeof(uint256_t)) == 0;
}

/**
 * 判断大数是否为0
 * @return 1为true，否则为false
 */
int uint256_is_zero(const uint256_t r) {
    return uint256_cmp(r, U256_0) == 0;
}

// 初始化点r为无穷远点
void uint256_point_init(u256_point_t* r) {
    memset(r, 0, sizeof(u256_point_t));
    uint256_set_mont_one(r->X);
    uint256_set_mont_one(r->Y);
}

/**
 * 设置点r的x及y坐标，点r为Jacobian加重射影坐标系
 * @param r 用于存储结果
 * @param x x坐标，仿射坐标
 * @param y y坐标，仿射坐标
 */
void uint256_point_set_xy(u256_point_t* r, const uint256_t x, const uint256_t y) {
    uint256_to_mont(r->X, x, GM_BN_P);
    uint256_to_mont(r->Y, y, GM_BN_P);
    uint256_set_mont_one(r->Z);
}

// 停留在蒙哥马利域
static void gm_point_get_xy_mont(const u256_point_t* p, uint256_t x, uint256_t y) {
    uint256_t z_inv;
    if (uint256_is_mont_one(p->Z)) {
        if (x) {
            uint256_copy(x, p->X);
        }
        if (y) {
            uint256_copy(y, p->Y);
        }
    }
    else {
        // z^{-1}
        uint256_inv(z_inv, p->Z, GM_BN_P);
        if (y) {
            uint256_mont_mul(y, p->Y, z_inv, GM_BN_P);
        }
        // z^{-2}
        uint256_sqr(z_inv, z_inv, GM_BN_P);
        if (x) {
            uint256_mont_mul(x, p->X, z_inv, GM_BN_P);
        }
        if (y) {
            uint256_mont_mul(y, y, z_inv, GM_BN_P);
        }
    }
}

/**
 * 获取点p的x坐标及y坐标
 * @param p Jacobian加重射影坐标系点p
 * @param x 存储x坐标，仿射坐标
 * @param y 存储y坐标，仿射坐标
 */
void uint256_point_get_xy(const u256_point_t* p, uint256_t x, uint256_t y) {
    gm_point_get_xy_mont(p, x, y);
    if (x) {
        uint256_from_mont(x, x, GM_BN_P);
    }
    if (y) {
        uint256_from_mont(y, y, GM_BN_P);
    }
}

/**
 * 判断p是否为无穷远点
 * @return 1为无穷远点，否则不是
 */
int uint256_is_at_infinity(const u256_point_t* p) {
    return uint256_is_zero(p->Z);
}

/**
 * 将二进制串转化为点
 * @param r 用于存储结果
 * @param in 二进制串
 */
void gm_point_from_bytes(u256_point_t* r, const uint8_t in[64]) {
    uint256_t x;
    uint256_t y;
    uint256_from_bytes(x, in);
    uint256_from_bytes(y, in + 32);
    uint256_point_set_xy(r, x, y);
}

// 4M+5S
/**
 * 倍点算法，r = p + p
 * @param r 用于存储结果
 * @param p 待计算点p
 */
void gm_point_double(u256_point_t* r, const u256_point_t* p) {
    uint256_t tmp1, tmp2;
    uint256_t X3, Y3, Z3;

    uint256_t R1, R2, R3;

    if (uint256_is_at_infinity(p)) {
        uint256_point_copy(r, p);
        return;
    }

    // λ1 = 3X^2 + aZ^4 = 3X^2 - 3Z^4 = 3 * ( (X + Z^2) * (X - Z^2) )
    // z^2
    uint256_sqr(tmp1, p->Z, GM_BN_P);
    // X + Z^2
    uint256_add(tmp2, p->X, tmp1, GM_BN_P);
    // X - Z^2
    uint256_sub(tmp1, p->X, tmp1, GM_BN_P);
    // (X + Z^2) * (X - Z^2)
    uint256_mont_mul(R1, tmp1, tmp2, GM_BN_P);
    // λ1 = 3 * (X + Z^2) * (X - Z^2)
    uint256_add(tmp1, R1, R1, GM_BN_P);
    uint256_add(R1, tmp1, R1, GM_BN_P);

    // λ2 = X4Y^2 = X * (2Y)^2
    // Z3 = 2YZ

    // 2Y
    uint256_add(tmp1, p->Y, p->Y, GM_BN_P);
    // Z3 = 2YZ
    uint256_mont_mul(Z3, tmp1, p->Z, GM_BN_P);

    // λ2 = X * (2Y)^2
    uint256_sqr(R2, tmp1, GM_BN_P);

    uint256_mont_mul(R2, p->X, R2, GM_BN_P);

    // λ3 = 8Y^4 = 2 * 4Y^4 = 2 * (2 * Y^2) ^ 2
    // Y^2
    uint256_sqr(tmp1, p->Y, GM_BN_P);
    // 2Y^2
    uint256_add(tmp1, tmp1, tmp1, GM_BN_P);
    // (2 * Y^2) ^ 2
    uint256_sqr(tmp1, tmp1, GM_BN_P);
    // λ3 = 2 * (2 * Y^2) ^ 2
    uint256_add(R3, tmp1, tmp1, GM_BN_P);

    // X3 = λ1^2 − 2λ2
    // R1^2
    uint256_sqr(tmp1, R1, GM_BN_P);
    // 2R2
    uint256_add(tmp2, R2, R2, GM_BN_P);
    // X3 = λ1^2 − 2λ2
    uint256_sub(X3, tmp1, tmp2, GM_BN_P);

    // Y3 = λ1(λ2 − X3) − λ3
    // λ2 − X3
    uint256_sub(tmp1, R2, X3, GM_BN_P);
    // λ1(λ2 − X3)
    uint256_mont_mul(tmp2, R1, tmp1, GM_BN_P);
    // Y3 = λ1(λ2 − X3) − λ3
    uint256_sub(Y3, tmp2, R3, GM_BN_P);

    // output
    uint256_copy(r->X, X3);
    uint256_copy(r->Y, Y3);
    uint256_copy(r->Z, Z3);
}

/**
 * 点加算法，r = a + b
 * @param r 用于存储结果
 * @param a 待计算点a
 * @param b 待计算点b
 */
void gm_point_add(u256_point_t* r, const u256_point_t* a, const u256_point_t* b) {
    // U1 = X1 * (Z2)^2
    // U2 = X2 * (Z1)^2
    // S1 = Y1 * (Z2)^3
    // S2 = Y2 * (Z1)^3
    // if U1==U2 && S1==S2 return double(a)
    // H = U2 - U1
    // R = S2 - S1
    // X3 = R^2 - H^3 - 2U1 * H^2
    // Y3 = R * ( U1 * H^2 - X3) - S1 * H^3
    // Z3 = H * Z1 * Z2

    uint256_t U1, U2;
    uint256_t S1, S2;
    uint256_t H;
    uint256_t R;

    uint256_t X3, Y3, Z3;

    if (uint256_is_at_infinity(a)) {
        uint256_point_copy(r, b);
        return;
    }

    if (uint256_is_at_infinity(b)) {
        uint256_point_copy(r, a);
        return;
    }

    // Z1 ^ 2
    uint256_sqr(H, a->Z, GM_BN_P);
    // Z2 ^ 2
    uint256_sqr(R, b->Z, GM_BN_P);
    // U1 = X1 * (Z2)^2
    uint256_mont_mul(U1, a->X, R, GM_BN_P);
    // U2 = X2 * (Z1)^2
    uint256_mont_mul(U2, b->X, H, GM_BN_P);
    // Z1 ^ 3
    uint256_mont_mul(H, H, a->Z, GM_BN_P);
    // Z2 ^ 3
    uint256_mont_mul(R, R, b->Z, GM_BN_P);
    // S1 = Y1 * (Z2)^3
    uint256_mont_mul(S1, a->Y, R, GM_BN_P);
    // S2 = Y2 * (Z1)^3
    uint256_mont_mul(S2, b->Y, H, GM_BN_P);

    if (uint256_cmp(U1, U2) == 0 && uint256_cmp(S1, S2) == 0) {
        gm_point_double(r, a);
        return;
    }

    // H = U2 - U1
    uint256_sub(H, U2, U1, GM_BN_P);
    // R = S2 - S1
    uint256_sub(R, S2, S1, GM_BN_P);
    // Z3 = H * Z1 * Z2
    uint256_mont_mul(Z3, H, a->Z, GM_BN_P);
    uint256_mont_mul(Z3, Z3, b->Z, GM_BN_P);
    // H ^ 2
    uint256_sqr(U2, H, GM_BN_P);
    // H ^ 3
    uint256_mont_mul(H, U2, H, GM_BN_P);
    // U1 * H ^2
    uint256_mont_mul(Y3, U1, U2, GM_BN_P);
    // 2 * U1 * H ^2
    uint256_add(U2, Y3, Y3, GM_BN_P);
    // R^2
    uint256_sqr(X3, R, GM_BN_P);
    // X3 = R^2 - H^3 - 2U1 * H^2
    uint256_sub(X3, X3, H, GM_BN_P);
    uint256_sub(X3, X3, U2, GM_BN_P);
    // Y3 = R * ( U1 * H^2 - X3) - S1 * H^3
    // ( U1 * H^2 - X3)
    uint256_sub(Y3, Y3, X3, GM_BN_P);
    // R * ( U1 * H^2 - X3)
    uint256_mont_mul(Y3, Y3, R, GM_BN_P);
    // S1 * H^3
    uint256_mont_mul(S1, S1, H, GM_BN_P);
    // Y3 = R * ( U1 * H^2 - X3) - S1 * H^3
    uint256_sub(Y3, Y3, S1, GM_BN_P);

    // output
    uint256_copy(r->X, X3);
    uint256_copy(r->Y, Y3);
    uint256_copy(r->Z, Z3);
}

/**
 * 多倍点算法，r = k[p]，即k个p相加
 * @param r 用于存储结果
 * @param k 大数k
 * @param p 待计算点p
 */
void gm_point_mul(u256_point_t* r, const uint256_t k, const u256_point_t* p) {
    char bits[257] = { 0 };
    u256_point_t _q;
    u256_point_t* q = &_q;
    int i;

    uint256_point_set_infinity(q);
    uint256_to_bits(k, bits);

    for (i = 0; i < 256; i++) {
        gm_point_double(q, q);
        if (bits[i] == '1') {
            gm_point_add(q, q, p);
        }
    }
    uint256_point_copy(r, q);
}

/**
 * 点解压缩
 * @param p 用于存储结果
 * @param in 未压缩的点（65字节）或压缩的点（33字节）
 */
void gm_point_decode(u256_point_t* p, const uint8_t* in) {
    if (in[0] == 0x04) {
        // 未压缩点，直接转化
        gm_point_from_bytes(p, in + 1);
    }
    else if (in[0] == 0x02 || in[0] == 0x03) {
        int yTile = in[0] - 0x02;
        uint256_t x, r;

        // 预计算的 (P + 1) / 4
        uint256_t padd1shit2 = {
                0x00000000, 0x40000000, 0xC0000000, 0xFFFFFFFF,
                0xFFFFFFFF, 0xFFFFFFFF, 0xBFFFFFFF, 0x3FFFFFFF };

        uint256_from_bytes(x, in + 1);
        uint256_to_mont(x, x, GM_BN_P);

        // r = x ^ 2
        uint256_sqr(r, x, GM_BN_P);

        // r = x^2 + a
        uint256_add(r, r, GM_BN_MONT_A, GM_BN_P);

        // r = x^3 + ax
        uint256_mont_mul(r, r, x, GM_BN_P);

        // r = x^3 + ax + b
        uint256_add(r, r, GM_BN_MONT_B, GM_BN_P);

        // r = sqrt(x^3 + ax + b) = (x^3 + ax + b) ^ ((P + 1) / 4) (mod P)
        uint256_exp(r, r, padd1shit2, GM_BN_P);

        uint256_from_mont(padd1shit2, r, GM_BN_P);
        if ((padd1shit2[0] & 0x01) != yTile) {
            uint256_sub(r, GM_BN_P, padd1shit2, GM_BN_P);
            uint256_to_mont(r, r, GM_BN_P);
        }

        uint256_copy(p->X, x);
        uint256_copy(p->Y, r);
        uint256_set_mont_one(p->Z);
    }
}

/**
 * SM2签名算法，方便单元测试
 * @param key SM2私钥
 * @param digest 消息摘要
 * @param sig 存储签名结果缓冲区
 * @param testK 随机数
 * @return 1如果签名成功，否则签名失败
 */
int gm_do_sign_for_test(const uint256_t key, const uint256_t digest, unsigned char sig[64], const uint256_t testK) {
    u256_point_t _P, * P = &_P;
    uint256_t d;
    uint256_t e;
    uint256_t k;
    uint256_t x;
    uint256_t r;
    uint256_t s;

    if (!key || !digest || !sig) {
        return -1;
    }

    uint256_to_mont(d, key, GM_BN_N);

    // e = H(M)
    uint256_copy(e, digest);

retry:
    if (NULL == testK) {
        // rand k in [1, n - 1]
        uint8_t buf[256];
        do {
            do {
                randombytes(buf, 256);
                uint256_from_bytes(k, buf);
            } while (uint256_cmp(k, GM_BN_N) >= 0);
        } while (uint256_is_zero(k));
    }
    else {
        uint256_copy(k, testK);
    }

    // (x, y) = kG
    gm_point_mul(P, k, GM_MONT_G_PTR);
    uint256_point_get_xy(P, x, NULL);


    // r = e + x (mod n)
    uint256_add(r, e, x, GM_BN_N);

    /* if r == 0 or r + k == n re-generate k */
    if (uint256_is_zero(r)) {
        goto retry;
    }
    uint256_add(x, r, k, GM_BN_N);
    if (uint256_is_zero(x)) {
        goto retry;
    }

    uint256_to_bytes(r, sig);

    /* s = ((1 + d)^-1 * (k - r * d)) mod n */
    uint256_to_mont(r, r, GM_BN_N);
    uint256_to_mont(k, k, GM_BN_N);

    uint256_mont_mul(e, r, d, GM_BN_N);
    uint256_sub(k, k, e, GM_BN_N);

    uint256_add(x, GM_BN_MONT_NONE, d, GM_BN_N);
    uint256_inv(x, x, GM_BN_N);
    uint256_mont_mul(s, x, k, GM_BN_N);

    if (uint256_is_zero(s)) {
        goto retry;
    }

    uint256_from_mont(s, s, GM_BN_N);
    uint256_to_bytes(s, sig + 32);
    return 1;
}

/**
 * SM2验签算法
 * @param key SM2私钥
 * @param digest 消息摘要
 * @param sig 签名结果，用于验签
 * @return 1如果验签成功，否则验签失败
 */
int gm_do_verify(const u256_point_t* key, const uint256_t digest, const unsigned char sig[64]) {
    u256_point_t _P, * P = &_P;
    u256_point_t _Q, * Q = &_Q;
    uint256_t r;
    uint256_t s;
    uint256_t e;
    uint256_t x;
    uint256_t t;

    if (!key || !digest || !sig) {
        return -1;
    }

    // parse signature values
    uint256_from_bytes(r, (const uint8_t*)sig);
    uint256_from_bytes(s, (const uint8_t*)sig + 32);
    if (uint256_is_zero(r) || uint256_cmp(r, GM_BN_N) >= 0 || uint256_is_zero(s) || uint256_cmp(s, GM_BN_N) >= 0) {
        return -1;
    }

    // parse public key
    uint256_point_copy(P, key);

    // t = r + s (mod n)
    // check t != 0
    uint256_add(t, r, s, GM_BN_N);
    if (uint256_is_zero(t)) {
        return -1;
    }

    // Q = s * G + t * P
    gm_point_mul(Q, s, GM_MONT_G_PTR);
    gm_point_mul(P, t, P);
    gm_point_add(Q, Q, P);
    uint256_point_get_xy(Q, x, NULL);

    // e  = H(M)
    // r' = e + x (mod n)
    uint256_copy(e, digest);
    uint256_add(e, e, x, GM_BN_N);

    // check if r == r'
    if (uint256_cmp(e, r) == 0) {
        return 1;
    }
    else {
        return 0;
    }
}

// SM2 a
static const unsigned char GM_ECC_A[] = {
        0xFF, 0xFF, 0xFF, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF,
        0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
        0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00,
        0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC };

// SM2 b
static const unsigned char GM_ECC_B[] = {
        0x28, 0xE9, 0xFA, 0x9E, 0x9D, 0x9F, 0x5E, 0x34,
        0x4D, 0x5A, 0x9E, 0x4B, 0xCF, 0x65, 0x09, 0xA7,
        0xF3, 0x97, 0x89, 0xF5, 0x15, 0xAB, 0x8F, 0x92,
        0xDD, 0xBC, 0xBD, 0x41, 0x4D, 0x94, 0x0E, 0x93 };

// SM2 Gx
static const unsigned char GM_ECC_G_X[] = {
        0x32, 0xC4, 0xAE, 0x2C, 0x1F, 0x19, 0x81, 0x19,
        0x5F, 0x99, 0x04, 0x46, 0x6A, 0x39, 0xC9, 0x94,
        0x8F, 0xE3, 0x0B, 0xBF, 0xF2, 0x66, 0x0B, 0xE1,
        0x71, 0x5A, 0x45, 0x89, 0x33, 0x4C, 0x74, 0xC7 };

// SM2 Gy
static const unsigned char GM_ECC_G_Y[] = {
        0xBC, 0x37, 0x36, 0xA2, 0xF4, 0xF6, 0x77, 0x9C,
        0x59, 0xBD, 0xCE, 0xE3, 0x6B, 0x69, 0x21, 0x53,
        0xD0, 0xA9, 0x87, 0x7C, 0xC6, 0x2A, 0x47, 0x40,
        0x02, 0xDF, 0x32, 0xE5, 0x21, 0x39, 0xF0, 0xA0 };

// SM2 N-1
static const uint256_t GM_BN_N_SUB_ONE = {
        0x39D54122, 0x53BBF409, 0x21C6052B, 0x7203DF6B,
        0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFE };

static const uint256_t GM_BN_2W = {
        0x00000000, 0x00000000, 0x00000000, 0x80000000,
        0x00000000, 0x00000000, 0x00000000, 0x00000000 };

static const uint256_t GM_BN_2W_SUB_ONE = {
        0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0x7FFFFFFF,
        0x00000000, 0x00000000, 0x00000000, 0x00000000 };

// 字转化为字节
#ifndef GM_PUT_UINT32_BE
#define GM_PUT_UINT32_BE(n, b, i)                   \
    {                                               \
        (b)[(i)] = (unsigned char) ((n) >> 24);     \
        (b)[(i) + 1] = (unsigned char) ((n) >> 16); \
        (b)[(i) + 2] = (unsigned char) ((n) >> 8);  \
        (b)[(i) + 3] = (unsigned char) ((n));       \
    }
#endif

/**
 * ZA计算，具体参照规范文档
 * @param id_bytes userid二进制串
 * @param idLen userid长度
 * @param pub_key 公钥
 * @param output 输出缓冲区
 */
void gm_sm2_compute_z_digest(const unsigned char* id_bytes, unsigned int idLen, const u256_point_t* pub_key,
    unsigned char output[32]) {
    gm_sm3_context _ctx;
    gm_sm3_context* ctx = &_ctx;
    uint256_t x, y;

    gm_sm3_init(ctx);

    // update idlen，这里的idlen是比特长度，所以移位注意一下
    ctx->buf[0] = ((idLen >> 5) & 0x0FF);
    ctx->buf[1] = ((idLen << 3) & 0x0FF);
    ctx->cur_buf_len = 2;

    // update id
    gm_sm3_update(ctx, id_bytes, idLen);

    // update a
    gm_sm3_update(ctx, GM_ECC_A, 32);

    // update b
    gm_sm3_update(ctx, GM_ECC_B, 32);

    // update Gx
    gm_sm3_update(ctx, GM_ECC_G_X, 32);

    // update Gy
    gm_sm3_update(ctx, GM_ECC_G_Y, 32);

    uint256_point_get_xy(pub_key, x, y);

    // update Px
    uint256_to_bytes(x, output);// 借用output当缓冲区
    gm_sm3_update(ctx, output, 32);

    // update Py
    uint256_to_bytes(y, output);// 借用output当缓冲区
    gm_sm3_update(ctx, output, 32);

    gm_sm3_done(ctx, output);
}

/**
 * 检查公钥是否合法
 * @param pub_key 公钥
 * @return 1合法，否则非法
 */
static int gm_sm2_check_public_key(const u256_point_t* pub_key) {
    if (uint256_is_at_infinity(pub_key)) {
        return 0;
    }
    uint256_t x, y, r;
    uint256_point_get_xy(pub_key, x, y);

    if (uint256_is_zero(x) || uint256_cmp(x, GM_BN_P) >= 0) {
        return 0;
    }

    if (uint256_is_zero(y) || uint256_cmp(y, GM_BN_P) >= 0) {
        return 0;
    }

    //y^2 = x^3 + ax + b
    uint256_to_mont(x, x, GM_BN_P);
    uint256_to_mont(y, y, GM_BN_P);

    // r = x ^ 2
    uint256_sqr(r, x, GM_BN_P);

    // r = x^2 + a
    uint256_add(r, r, GM_BN_MONT_A, GM_BN_P);

    // r = x^3 + ax
    uint256_mont_mul(r, r, x, GM_BN_P);

    // r = x^3 + ax + b
    uint256_add(r, r, GM_BN_MONT_B, GM_BN_P);

    uint256_sqr(y, y, GM_BN_P);

    if (uint256_cmp(r, y) != 0) {
        return 0;
    }

    return 1;
}

/**
 * 恢复私钥
 * @param ctx SM2上下文
 * @param key 私钥
 * @param kLen 长度必须为32
 */
static int recover_private_key(gm_sm2_context* ctx, const unsigned char* key, unsigned int kLen) {
    if (kLen != 32) {
        return 0;
    }
    uint256_from_bytes(ctx->private_key, key);
    // check k ∈ [1, n-2]
    if (uint256_is_zero(ctx->private_key) || uint256_cmp(ctx->private_key, GM_BN_N_SUB_ONE) >= 0) {
        return 0;
    }
    // check public key
    gm_point_mul(&ctx->public_key, ctx->private_key, GM_MONT_G_PTR);
    if (gm_sm2_check_public_key(&ctx->public_key) != 1) {
        return 0;
    }
    return 1;
}

/**
 * 恢复公钥
 * @param ctx SM2上下文
 * @param key 公钥PC||x||y或者yTile||x
 * @param kLen 公钥长度必须为33或65
 */
static int recover_public_key(gm_sm2_context* ctx, const unsigned char* key, unsigned int kLen) {
    if ((kLen != 33 && kLen != 65) || (key[0] != 0x04 && key[0] != 0x02 && key[0] != 0x03)) {
        return 0;
    }
    // check public key
    gm_point_decode(&ctx->public_key, key);
    if (gm_sm2_check_public_key(&ctx->public_key) != 1) {
        return 0;
    }
    return 1;
}

/**
 * 签名验签初始化
 * @param ctx SM2上下文
 * @param key 公钥PC||x||y或者yTile||x用于验签，私钥用于签名
 * @param kLen 公钥长度必须为33或65，私钥为32字节
 * @param id_bytes userid二进制串
 * @param idLen userid长度
 * @param forSign 1为签名，否则为验签
 * @return 1返回成功，否则为密钥非法
 */
int gm_sm2_sign_init(gm_sm2_context* ctx, const unsigned char* key, unsigned int kLen,
    const unsigned char* id_bytes, unsigned int idLen, int forSign) {
    if (forSign) {
        // 私钥签名
        if (recover_private_key(ctx, key, kLen) == 0) {
            return 0;
        }
    }
    else {
        // 公钥验签
        if (recover_public_key(ctx, key, kLen) == 0) {
            return 0;
        }
    }
    // compute z digest
    gm_sm2_compute_z_digest(id_bytes, idLen, &ctx->public_key, ctx->buf);
    gm_sm3_init(&ctx->sm3_ctx);
    gm_sm3_update(&ctx->sm3_ctx, ctx->buf, 32);
    ctx->state = forSign;

    return 1;
}

/**
 * 添加待签名验签数据
 * @param ctx SM2上下文
 * @param input 待处理数据
 * @param iLen 待处理数据长度
 */
void gm_sm2_sign_update(gm_sm2_context* ctx, const unsigned char* input, unsigned int iLen) {
    gm_sm3_update(&ctx->sm3_ctx, input, iLen);
}

int gm_sm2_sign(gm_sm2_context* ctx, unsigned char sig[64], const uint256_t testKey) {
    uint256_t digest;

    gm_sm3_done(&ctx->sm3_ctx, ctx->buf);

    uint256_from_bytes(digest, ctx->buf);

    if (ctx->state) {
        return gm_do_sign_for_test(ctx->private_key, digest, sig, testKey);
    }
    else {
        return gm_do_verify(&ctx->public_key, digest, sig);
    }
}
